#include "map_location.hpp"

void odometry_callback(const nav_msgs::Odometry &current_info)
{
    current_point.x= current_info.pose.pose.position.x;
    current_point.y= current_info.pose.pose.position.y;
    current_point.z= current_info.pose.pose.position.z;
    geometry_msgs::Quaternion current_angle_temp;
    current_angle_temp.x= current_info.pose.pose.orientation.x;
    current_angle_temp.y= current_info.pose.pose.orientation.y;
    current_angle_temp.z= current_info.pose.pose.orientation.z;
    current_angle_temp.w= current_info.pose.pose.orientation.w;
    geometry_msgs::Vector3 curr_angle_temp = map_location_core->toEulerAngle(current_angle_temp);
    current_angle = tf::createQuaternionMsgFromRollPitchYaw(curr_angle_temp.x,curr_angle_temp.y-0.785,curr_angle_temp.z);
    curr_angle = map_location_core->toEulerAngle(current_angle);

    geometry_msgs::Point rotation_hehe_data = map_location_core->rotation_hehe(curr_angle, -0.1, 0.0, 0.03);
    current_point.x += rotation_hehe_data.x;
    current_point.y += rotation_hehe_data.y;
    current_point.z += rotation_hehe_data.z;

    current_yaw_angle = curr_angle.z;

    //add offsets
    geometry_msgs::Vector3 pose_map;
    geometry_msgs::Quaternion Quaternion_map;
    pose_map.x = current_point.x +  P_offset.x;
    pose_map.y = current_point.y +  P_offset.y;
    pose_map.z = current_point.z +  P_offset.z;
    Quaternion_map = map_location_core->quatMultiply(current_angle, Q_offset);

    geometry_msgs::PoseStamped pose_pub;
    seq_cnt++;
    pose_pub.header.seq = seq_cnt;
    pose_pub.header.frame_id = "map";
    pose_pub.pose.position.x = pose_map.x;
    pose_pub.pose.position.y = pose_map.y;
    pose_pub.pose.position.z = pose_map.z;
    pose_pub.pose.orientation.x = Quaternion_map.x;
    pose_pub.pose.orientation.y = Quaternion_map.y;
    pose_pub.pose.orientation.z = Quaternion_map.z;
    pose_pub.pose.orientation.w = Quaternion_map.w;

    

    //pub odom 
    nav_msgs::Odometry odom_pub;
    odom_pub = current_info;
    odom_pub.pose.pose.position.x = pose_pub.pose.position.x;
    odom_pub.pose.pose.position.y = pose_pub.pose.position.y;
    odom_pub.pose.pose.position.z = pose_pub.pose.position.z;
    odom_pub.pose.pose.orientation.x = pose_pub.pose.orientation.x;
    odom_pub.pose.pose.orientation.y = pose_pub.pose.orientation.y;
    odom_pub.pose.pose.orientation.z = pose_pub.pose.orientation.z;
    odom_pub.pose.pose.orientation.w = pose_pub.pose.orientation.w;
    
    
    if(tag_detected_flag)
    {
        map_location_pub.publish(pose_pub);
        map_odom_pub.publish(odom_pub);
    }

    std_msgs::Bool tag_f;
    tag_f.data = tag_detected_flag;
    tag_flag_pub.publish(tag_f);


}
 
void tagdetect_callback(const nav_msgs::Odometry &msg)
{
    int tag_level_temp = (int)msg.pose.covariance[0];
    Q_tag_world =tf:: createQuaternionMsgFromRollPitchYaw(0,0,tag_array[tag_level_temp*4+3]);
    R_tag_world= map_location_core->Quaternion2RotationMatrix(Q_tag_world.x, Q_tag_world.y, Q_tag_world.z, Q_tag_world.w);
    P_tag_world.x = tag_array[tag_level_temp*4];
    P_tag_world.y = tag_array[tag_level_temp*4+1];
    P_tag_world.z = tag_array[tag_level_temp*4+2];

    geometry_msgs::Point P_cam_tag;
    geometry_msgs::Quaternion Q_cam_tag;
    geometry_msgs::Point P_plane_home = current_point;
    geometry_msgs::Quaternion Q_plane_home = current_angle;

    P_cam_tag.x= msg.pose.pose.position.x;
    P_cam_tag.y= msg.pose.pose.position.y;
    P_cam_tag.z= msg.pose.pose.position.z;


    Q_cam_tag.x= msg.pose.pose.orientation.x;
    Q_cam_tag.y= msg.pose.pose.orientation.y;
    Q_cam_tag.z= msg.pose.pose.orientation.z;
    Q_cam_tag.w= msg.pose.pose.orientation.w;

    Eigen::Matrix3d R_cam_tag= map_location_core->Quaternion2RotationMatrix(Q_cam_tag.x, Q_cam_tag.y, Q_cam_tag.z, Q_cam_tag.w);

    P_offset = map_location_core->calc_P_offset(R_cam_tag, P_cam_tag, P_plane_cam, P_plane_home, R_tag_world, P_tag_world);

    Q_offset = map_location_core->calc_Q_offset(Q_cam_tag, Q_tag_world, Q_plane_cam, Q_plane_home);

    tag_detected_flag = true;
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "map_location");
    ros::NodeHandle nh;
    map_location_core->detect_serial_number();
    //3x3=9 tags, 
    nh.param<double>("tag_x_0",tag_x_0,0.0);
    nh.param<double>("tag_y_0",tag_y_0,0.0);
    nh.param<double>("tag_z_0",tag_z_0,0.0);
    nh.param<double>("tag_yaw_0",tag_yaw_0,0.0);

    nh.param<double>("tag_x_1",tag_x_1,0.0);
    nh.param<double>("tag_y_1",tag_y_1,0.0);
    nh.param<double>("tag_z_1",tag_z_1,0.0);
    nh.param<double>("tag_yaw_1",tag_yaw_1,0.0);

    nh.param<double>("tag_x_2",tag_x_2,0.0);
    nh.param<double>("tag_y_2",tag_y_2,0.0);
    nh.param<double>("tag_z_2",tag_z_2,0.0);
    nh.param<double>("tag_yaw_2",tag_yaw_2,0.0);

    nh.param<double>("tag_x_3",tag_x_3,0.0);
    nh.param<double>("tag_y_3",tag_y_3,0.0);
    nh.param<double>("tag_z_3",tag_z_3,0.0);
    nh.param<double>("tag_yaw_3",tag_yaw_3,0.0);

    nh.param<double>("tag_x_4",tag_x_4,0.0);
    nh.param<double>("tag_y_4",tag_y_4,0.0);
    nh.param<double>("tag_z_4",tag_z_4,0.0);
    nh.param<double>("tag_yaw_4",tag_yaw_4,0.0);

    nh.param<double>("tag_x_5",tag_x_5,0.0);
    nh.param<double>("tag_y_5",tag_y_5,0.0);
    nh.param<double>("tag_z_5",tag_z_5,0.0);
    nh.param<double>("tag_yaw_5",tag_yaw_5,0.0);

    nh.param<double>("tag_x_6",tag_x_6,0.0);
    nh.param<double>("tag_y_6",tag_y_6,0.0);
    nh.param<double>("tag_z_6",tag_z_6,0.0);
    nh.param<double>("tag_yaw_6",tag_yaw_6,0.0);

    nh.param<double>("tag_x_7",tag_x_7,0.0);
    nh.param<double>("tag_y_7",tag_y_7,0.0);
    nh.param<double>("tag_z_7",tag_z_7,0.0);
    nh.param<double>("tag_yaw_7",tag_yaw_7,0.0);

    nh.param<double>("tag_x_8",tag_x_8,0.0);
    nh.param<double>("tag_y_8",tag_y_8,0.0);
    nh.param<double>("tag_z_8",tag_z_8,0.0);
    nh.param<double>("tag_yaw_8",tag_yaw_8,0.0);

    tag_array[0] = tag_x_0;
    tag_array[1] = tag_y_0;
    tag_array[2] = tag_z_0;
    tag_array[3] = tag_yaw_0;

    tag_array[4] = tag_x_1;
    tag_array[5] = tag_y_1;
    tag_array[6] = tag_z_1;
    tag_array[7] = tag_yaw_1;
    
    tag_array[8] = tag_x_2;
    tag_array[9] = tag_y_2;
    tag_array[10] = tag_z_2;
    tag_array[11] = tag_yaw_2;

    tag_array[12] = tag_x_3;
    tag_array[13] = tag_y_3;
    tag_array[14] = tag_z_3;
    tag_array[15] = tag_yaw_3;

    tag_array[16] = tag_x_4;
    tag_array[17] = tag_y_4;
    tag_array[18] = tag_z_4;
    tag_array[19] = tag_yaw_4;

    tag_array[20] = tag_x_5;
    tag_array[21] = tag_y_5;
    tag_array[22] = tag_z_5;
    tag_array[23] = tag_yaw_5;

    tag_array[24] = tag_x_6;
    tag_array[25] = tag_y_6;
    tag_array[26] = tag_z_6;
    tag_array[27] = tag_yaw_6;

    tag_array[28] = tag_x_7;
    tag_array[29] = tag_y_7;
    tag_array[30] = tag_z_7;
    tag_array[31] = tag_yaw_7;

    tag_array[32] = tag_x_8;
    tag_array[33] = tag_y_8;
    tag_array[34] = tag_z_8;
    tag_array[35] = tag_yaw_8;


    //camera T
    Q_plane_cam =tf:: createQuaternionMsgFromRollPitchYaw(0,-89.9*deg2rad,0);
    R_plane_cam= map_location_core->Quaternion2RotationMatrix(Q_plane_cam.x, Q_plane_cam.y, Q_plane_cam.z, Q_plane_cam.w);
    P_plane_cam.x = 0.0;
    P_plane_cam.y = 0.0;
    P_plane_cam.z = 0.0;

    //init offsets
    P_offset.x = 0;
    P_offset.y = 0;
    P_offset.z = 0;
    Q_offset.x = 0;
    Q_offset.y = 0;
    Q_offset.z = 0;
    Q_offset.w = 1;

    odometrysub     = nh.subscribe("/camera/odom/sample",10,odometry_callback);
    tagdect_sub = nh.subscribe("/tag_Odometry",10, tagdetect_callback);

    map_location_pub = nh.advertise<geometry_msgs::PoseStamped>("/map_location/pose",10);
    map_odom_pub = nh.advertise<nav_msgs::Odometry>("/map_location/odom",10);
    tag_flag_pub = nh.advertise<std_msgs::Bool>("/tag_flag",10);

    ros::Rate rate(100);
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}




